#include "CollisionDetectionWorkCollector.h"

namespace coolpool
{
	CollisionDetectionWorkCollector::CollisionDetectionWorkCollector()
	{
		work_list_ = new CollisionPairsQueue();
	}

	CollisionDetectionWorkCollector::~CollisionDetectionWorkCollector()
	{
		delete work_list_;
	}

	CollisionPair CollisionDetectionWorkCollector::getCollisionPair()
	{
		assert(!work_list_->empty());
		CollisionPair output = work_list_->front();
		work_list_->pop();
		return output;
	}

	void CollisionDetectionWorkCollector::enqueueCollisionSynchronized(const Collision * col)
	{
		boost::mutex::scoped_lock sc_lock(lock_read_);
		// No collision.
		if (!col)
			return;
		// Valid collision.
		else
		{
			switch (col->type)
			{
				case INTERPENETRATION:
				{
					// If the firsts vector is empty it means the current col is becoming the first.
					// If the col collision time is smaller then the firsts, we have a new first collision.
					if (firsts_->empty() || *col < *((*firsts_)[0]))
					{
						// Remove the current firsts from the heap.
						for (unsigned i = 0; i < firsts_->size(); ++i)
						{
							delete (*firsts_)[i];
						}

						// Reinits the firsts list.
						firsts_->clear();
						firsts_->push_back(col);
					}
					// Occasionally happens that even with finer step there will be more collisions at the time.
					else if (*col == *((*firsts_)[0]))
					{
						firsts_->push_back(col);
					}
					else
					{
						delete col;
					}
					return;
				}
				case RESTING_CONTACT:
				{
					resting_contact_->push_back(col);
					return;
				}
				case MOVINGAWAY:
				{
					delete col;
					// Collision was already handled.
					return;
				}
				default:
					// Can not happen.
					assert(false);
					return;
			}
		}
	}
}
